#include "WProgram.h"
//This is a Servo test
#include <Servo.h>
#include <AFSoftSerial.h>
void setup();
void loop();
void selectLineOne();
void selectLineTwo();
void clearLCD();
void backlightOn();
void backlightOff();
void serCommand();
Servo servo1;
Servo servo2;
int servo1_port = 14;
int servo2_port = 15;
AFSoftSerial mySerial =  AFSoftSerial(6, 7);
void setup()
{
  
    mySerial.begin(9600);
  backlightOn();
  delay(200);
  mySerial.print("YO bitch!");
  delay(1000);
  backlightOff();
  delay(50);
  servo1.attach(servo1_port);
  servo1.setMaximumPulse(2200);
  delay(40);
  servo2.attach(servo2_port);
  servo2.setMaximumPulse(2200);
  delay(40);
  Serial.begin(9600);
  delay(40);
  Serial.print("Ready");
}

void loop()
{


  if ( Serial.available()) {
    char ch = Serial.read();

    switch(ch) {
      case 's':
        servo2.detach();
        servo1.detach();
        delay(40);
        servo2.attach(servo2_port);
        servo1.attach(servo1_port);
        delay(40);
        servo1.write(135);
        servo2.write(45);
 
        break;
      case 'w':
        servo2.detach();
        servo1.detach();
        delay(40);
        servo2.attach(servo2_port);
        servo1.attach(servo1_port);
        delay(40);
        servo1.write(45);
        servo2.write(135);

        break;
      case 'd':
        servo2.detach();
        servo1.detach();
        delay(40);
        servo2.attach(servo2_port);
        servo1.attach(servo1_port);
        delay(40);
        servo1.write(135);
        servo2.write(135);

        break;
      case 'a':
        servo2.detach();
        servo1.detach();
        delay(40);
        servo2.attach(servo2_port);
        servo1.attach(servo1_port);
        delay(40);
        servo1.write(45);
        servo2.write(45);
  
        break;
        //STOP FUNCTION
  case 'x':
        servo2.detach();
        servo1.detach();
        delay(40);
        servo2.attach(servo2_port);
        servo1.attach(servo1_port);
        delay(40);
        break;
        
    }
  }

  Servo::refresh();
}

void selectLineOne(){  //puts the cursor at line 0 char 0.
  mySerial.print(0xFE, BYTE);   //command flag
  mySerial.print(128, BYTE);    //position
}
void selectLineTwo(){  //puts the cursor at line 0 char 0.
  mySerial.print(0xFE, BYTE);   //command flag
  mySerial.print(192, BYTE);    //position
}
void clearLCD(){
  mySerial.print(0xFE, BYTE);   //command flag
  mySerial.print(0x01, BYTE);   //clear command.
}
void backlightOn(){  //turns on the backlight
  mySerial.print(0x7C, BYTE);   //command flag for backlight stuff
  mySerial.print(157, BYTE);    //light level.
}
void backlightOff(){  //turns off the backlight
  mySerial.print(0x7C, BYTE);   //command flag for backlight stuff
  mySerial.print(128, BYTE);     //light level for off.
}
void serCommand(){   //a general function to call the command flag for issuing all other commands   
  mySerial.print(0xFE, BYTE);
}



int main(void)
{
	init();

	setup();
    
	for (;;)
		loop();
        
	return 0;
}

